#!/usr/bin/env python2
#coding=utf-8
import math 
import numpy as np 
import rospy
#from std_msgs.msg import std_msgs
#from plot_py.msg import INS_Data
from sensor_msgs.msg import NavSatFix
import time
from matplotlib.animation import FuncAnimation
import os
from pyproj import Proj, transform
a = 6378137. #长轴
f = 1 / 298.257223563 #扁率
fle = open("/home/fxy/datasets/GPS.txt",mode = 'w')

def gps_to_cartesion(latitude,longitude,altitude):
    # 定义经纬度和高度坐标系
    wgs84 = Proj(proj='latlong', datum='WGS84')
    xyz = Proj(proj='geocent', datum='WGS84')

    # 将经纬度高度转换为xyz坐标
    X,Y,Z = transform(wgs84, xyz, longitude, latitude, altitude, radians=False)

    return X,Y,Z

def callback(data):
    
    x,y,z = gps_to_cartesion(data.latitude,data.longitude,data.altitude)
    stamp = data.header.stamp
    time = float(stamp.secs) + (stamp.nsecs / 1e+9 ) 
    fle.write(str(time) +" "+str(x) +" "+str(y) + " " + str(z))
    fle.write(" 0. 0. 0. 0.\n")
    # print(x,y,z)
    print(str(time) +" "+str(x) +" "+str(y) + " " + str(z))
    print("\n")

if __name__ == '__main__':
    print("init")
    rospy.init_node('GPS',anonymous=True) 
    print("init success")
    rospy.Subscriber("/chcnav_fix_demo/fix",NavSatFix,callback)
    rospy.spin() 
    fle.close()
